#include "datakeyboard.h"
#include "CaliCam.hpp"
#include <vector>
#include <QApplication>
#include <QEvent>
#include <QKeyEvent>
#include <QDebug>
#include <cctype>
#include <vector>



using namespace cv;
using namespace std;



#ifdef RECORD_DATA

auto start_time = std::chrono::high_resolution_clock::now();



void DataKeyboard::initFilePath()
{
    videoNumsPath = dataDirPath + "/" + "videonums.txt";

    videoNumsFp = fopen(videoNumsPath.c_str(), "r");
    if(!videoNumsFp) {
        throw "Fail to Open videoNumsFile";
    }
    int videoNum = 0;
    fscanf(videoNumsFp, "%d", &videoNum);
    fclose(videoNumsFp);
    videoNumsFp = NULL;
    std::string videoNumStr = std::to_string(videoNum);
    timeLineFilePath = dataDirPath + "/" +  "timeline_" + videoNumStr + ".txt" ;
    tipslineFilePath = dataDirPath + "/" + "tipsline_" + videoNumStr + ".txt";
    tipsPointsFilePath = dataDirPath + "/" + "tippoints_" + videoNumStr + ".txt";
    videoPath = dataDirPath + "/" + "video_" + videoNumStr + ".avi";
    timelineFp = fopen(timeLineFilePath.c_str(), "w");
    tipslineFp = fopen(tipslineFilePath.c_str(), "w");
    tipsPointsFp = fopen(tipsPointsFilePath.c_str(), "w");
    if(!timelineFp || !tipslineFp || !tipsPointsFp)
        throw std::ios_base::failure("can not create file");
#ifdef WRITE_VIDEO
    videowriter.open(videoPath, VideoWriter::fourcc('M', 'J', 'P', 'G'), rate, frameSize);
    for(int i = 0; i < 1100; ++i)
        videoBuffer[i] = Mat::zeros(CAM_HEIGHT, CAM_WIDTH, CV_8UC3);
#endif

}

void setStartTime()
{
    start_time = std::chrono::high_resolution_clock::now();
}

void DataKeyboard::updateTimeLine(const std::vector<int64>& timelines) {
    for(size_t i = 0; i < timelines.size(); ++i)
        fprintf(timelineFp, "%lld\n", timelines[i]);
}

void DataKeyboard::appendTimeLine(int64 time) {
    fprintf(timelineFp, "%lld\n", time);
}

void DataKeyboard::appendCurrentTime() {
    auto end_time = std::chrono::high_resolution_clock::now();
    int64 durTime = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
    appendTimeLine(durTime);
}

void DataKeyboard::initTipsLine(int n) {
    fprintf(tipslineFp, "%d\n", n);
}

void DataKeyboard::appendTipsLine(int frame1, int frame2) {
    fprintf(tipslineFp, "%d %d\n", frame1, frame2);
}

void DataKeyboard::appendTipsPoints(int x, int y) {
    fprintf(tipsPointsFp, "%d %d\n", x, y);
}

void DataKeyboard::incVideoNum()
{
    videoNumsFp = fopen(videoNumsPath.c_str(), "r");
    if(!videoNumsFp) {
        throw "Fail to Open videoNumsFile";
    }
    int videoNum = 0;
    fscanf(videoNumsFp, "%d", &videoNum);
    fclose(videoNumsFp);
    videoNumsFp = fopen(videoNumsPath.c_str(), "w");
    fprintf(videoNumsFp, "%d", ++videoNum);
    fclose(videoNumsFp);
}

void DataKeyboard::closeFile()
{
    fclose(timelineFp);
    fclose(tipslineFp);
    fclose(tipsPointsFp);
}

void DataKeyboard::writeIntoFile()
{
#ifdef WRITE_VIDEO
    for(int i = 0; i < videoFrameNum; ++i) {
        videowriter.write(videoBuffer[i]);
    }
    videowriter.release();
#endif
    for(size_t i = 0; i < frameId.size(); ++i) {
        appendTimeLine((int64)(frameId[i] / rate * 1000));
    }
    initTipsLine(allTips.size());
    size_t tipsNum = 0;
    for(size_t i = 0; i < allTips.size(); ++i) {
        appendTipsLine(tipsNum, tipsNum + allTips[i].size());
        tipsNum += allTips[i].size();
    }
}



#endif




DataKeyboard::DataKeyboard(LaserKeyboard *parent) : LaserKeyboard(parent)
{
    UpCallBack *upCallBack = new UpCallBack();
    connect(upCallBack, SIGNAL(gestureSignal(int)), this, SLOT(triggerGestureSignal(int)));
    connect(upCallBack, SIGNAL(pointSignal(float, float)), this, SLOT(triggerKeyTappedSignal(float, float)));
    //this->touchManager.setTouchDownCallBack(new DownCallBack());
    this->touchManager.setTouchUpCallBack(upCallBack);
}

#ifdef USE_NEW_GESTURE
void DataKeyboard::triggerGestureSignal(int k) {
    emit gestureTriggered(k);
}
#endif



template <class T>
static T getDis(T x1, T y1, T x2, T y2) {
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

void DataKeyboard::run()
{
#ifdef RECORD_DATA
    initFilePath();
#endif
    findTapPos(1);
#ifdef RECORD_DATA
    incVideoNum();
    closeFile();
#endif
}

static int getAvgBright(const cv::Mat src)
{
    int64 totalBright = 0, totalPixel = 0;
    for(int i = 0; i < src.rows; i++)
        for(int j = 0; j < src.cols; j++) {
            if (DataKeyboard::isInSquareMask(cv::Point(j, i))) {
                int tempB = src.at<uchar>(i, j);
                totalPixel++;
                totalBright += tempB;
            }

        }
    return totalBright / totalPixel;
}

static bool inImgRange(int x, int y, int rows, int cols)
{
    return x >= 0 && x < rows && y >= 0 && y < cols;
}

int getAvgSquareBright(int cx, int cy, const cv::Mat grayMat, int len = 10)
{
    int64 centerBright = 0, centerPixels = 0;
    for(int i = -len; i < len; ++i) {
        for(int j = -len; j < len; ++j) {
            int uu = cy + i, vv = cx + j;
            if(inImgRange(uu,vv, grayMat.rows, grayMat.cols) && grayMat.at<uchar>(uu, vv) > 0) {
                centerBright += grayMat.at<uchar>(uu, vv);
                centerPixels++;
            }
        }
    }
    if(centerPixels == 0)
        return 0;
    return static_cast<int>(centerBright / centerPixels);
}

static std::vector<float> normalize(const std::vector<float>& a)
{
    std::vector<float> b;
    float minNum = *std::min_element(a.begin(), a.end()), maxNum = *std::max_element(a.begin(), a.end());
    float range = maxNum - minNum;
    for(std::vector<float>::const_iterator it = a.cbegin(); it != a.cend(); it++) {
        b.push_back((*it - minNum) / range);
    }
    return b;
}

#define USE_MOMENT

static  inline cv::Point getWC(const Contour& c)
{
#ifndef USE_MOMENT
    int total_x = 0, total_y = 0;
    for(auto it = c.begin(); it != c.end(); it++) {
        total_x += it->x, total_y += it->y;
    }
    return cv::Point(total_x / c.size(), total_y / c.size());
#else
    Moments mu = moments(c);
    int xc = mu.m10 / mu.m00, yc = mu.m01 / mu.m00;
    return cv::Point(xc, yc);
#endif
}

bool DataKeyboard::judgeTap(const Contour & c, double area, const cv::Mat grayMat)
{
    if(area < 1e-8)
            return 0;
    cv::Point cp = getWC(c);
    int xc = cp.x, yc = cp.y;
    auto t1_1 = std::chrono::high_resolution_clock::now();
    Point3f realP = getWorldPos(xc, yc);
    //auto t2 = std::chrono::high_resolution_clock::now();
    double y = realP.z, x = -realP.y;
    int upBright1 = getAvgSquareBright(xc, yc - 10, grayMat, 4), upBright2 = getAvgSquareBright(xc, yc - 15, grayMat, 4);
    int centerBright = getAvgSquareBright(xc, yc, grayMat,2), leftBright1 = getAvgSquareBright(xc - 5, yc - 3,  grayMat, 2), rightBright1 = getAvgSquareBright(xc + 5, yc - 3, grayMat, 2);
    int minBright = min(upBright1, min(upBright2, min(centerBright, min(leftBright1, rightBright1 ))));
    int maxBright = max(upBright1, max(upBright2, max(centerBright, max(leftBright1, rightBright1 ))));
    int brightRange = maxBright - minBright;
    //std::cout << "upbright1 is " << upBright1 << " upbright2 is " << upBright2 << " center bright is " << centerBright << " left Bright " << leftBright1 << "right bright " << rightBright1 << std::endl;
    if(upBright1 > centerBright - 10)
        return 0;
    else {
        //std::cout << "bright " << upBright1 - minBright << " " << upBright2 - minBright << " " << centerBright - minBright << " " << leftBright1 - minBright << " " << rightBright1 - minBright << std::endl;
        std::vector<float> brightV;
        brightV.push_back(upBright1), brightV.push_back(upBright2), brightV.push_back(centerBright), brightV.push_back(leftBright1), brightV.push_back(rightBright1);
       // appendBrightLine(normalizeBright(<#int bright#>, <#int minBright#>, <#int#>), upBright2 - minBright, <#int centerBright#>, <#int leftBright#>, <#int rightBright#>)
        brightV = normalize(brightV);
#ifdef RECORD_DATA
        if(area > 80 && centerBright > min(bgBright + 70, 180))
            appendBrightLine(brightV);
#endif
#ifdef LOG_BRIGHT_INFO
        std::cout << "frame " << totalFrame << " area is " << area << " " << "center bright is " << centerBright;
        std::cout << " bright is ";
        for(int i = 0; i < 5; ++i)
            std::cout << brightV[i] << " ";
        std::cout <<  std::endl;

#endif

        /*
        auto t3 = std::chrono::high_resolution_clock::now();

        long dur0 = cast_t(t1_1 - t1), dur1 = cast_t(t2 - t1_1), dur2 = cast_t(t3 - t2);
        long totalDur = dur0 + dur1 + dur2;
        total_mu += dur0;
        //std::cout << "judge dur " << dur0 *1.0 / totalDur << " " << dur1 *1.0 / totalDur << " " << dur2 * 1.0 / totalDur << std::endl;

        static long lastMod = 0;
        if(total_mu / 1000000000 > lastMod) {
            lastMod = total_mu / 1000000000;
            std::cout << "mu consumes " << total_mu * 1.0 / 1000000000 << "s\n";
        }
        */

        if(!(centerBright > min(bgBright + 50, 180) &&  brightV[0] > 0 && brightV[0] < 0.8 && brightV[1] < 1e-6 && brightV[2] > 0.6 && brightV[3] > 0.4 && brightV[4] > 0.3))
                    return 0;

        return area < 15000 && area > 120;
    }

    //return area > -2.0495 * y + 200;

}

bool DataKeyboard::inLeftRegion(cv::Point p)
{
    Point3f realP = this->getWorldPos(p.x, p.y);
    float x = - realP.y, y = realP.z;
    return (x >= 2 && x <= 128 && y >= 25 && y <= 74);
}

bool DataKeyboard::inRightRegion(cv::Point p)
{
    Point3f realP = this->getWorldPos(p.x, p.y);
    float x = - realP.y, y = realP.z;
    return (x >= 128 && x <= 243 && y >= 25 && y <= 74);
}

bool DataKeyboard::inInputRegion(cv::Point p)
{
    Point3f realP = this->getWorldPos(p.x, p.y);
    float x = - realP.y, y = realP.z;
    return (x >= 3 && x <= 247 && y >= 26 && y <= 72);
}

bool DataKeyboard::isDeleteKey(cv::Point p)
{
    Point3f realP = this->getWorldPos(p.x, p.y);
    float x = - realP.y, y = realP.z;
    return (x >= 202 && x <= 232 && y >= 1 && y <= 24);
}

void DataKeyboard::triggerKeyTappedSignal(float x, float y)
{
    qDebug() << "trigger tap " << x << " " << y;
    emit keyTapped(x, y);
}

#define MIN_BRIGHT_SPLIT 40

static  int getkthBright(const cv::Mat &src, float k) {
    std::vector<int32_t> allBright;
    uint64_t totalPixel = 0;
    int minBright = 9999, maxBright = 0;
    for(int i = 0; i < src.rows; i++)
        for(int j = 0; j < src.cols; j++) {
            if (DataKeyboard::isInSquareMask(cv::Point(j, i))) {
                int32_t tempB = src.at<uchar>(i, j);
                minBright = min(minBright, tempB);
                maxBright = max(maxBright, tempB);
                totalPixel++;
                allBright.push_back(tempB);
            }


    }
    size_t kIndex = (size_t)(totalPixel * k);
    std::cout << "max bright - min bright = " <<maxBright - minBright << std::endl;
    if (maxBright - minBright > MIN_BRIGHT_SPLIT) {
        std::sort(allBright.begin(), allBright.end());
        std::cout << " th bright is ";
        for (float x = 0.0; x <= 1.0; x += 0.01) {
            std::cout << allBright[totalPixel * x] << " ";
        }
        std::cout << std::endl;
        return allBright[kIndex];
    }
    return maxBright;

}

int DataKeyboard::handleImg(Mat &src, double fps)
{
    //this->bgBright = 120;
    Mat grayDst, dst;
    int tempAvgBright = 0;
    cvtColor(src, grayDst, COLOR_BGR2GRAY);
    if(totalFrame < BG_BRIGHT_EPOCH) {
//        tempAvgBright = getAvgBright(grayDst);
        tempAvgBright = getkthBright(grayDst, 0.9);
        qDebug() << "Avg bright is " << tempAvgBright;
    }
//    GaussianBlur(grayDst, dst, Size(5, 5), 10, 10);
    if(totalFrame < BG_BRIGHT_EPOCH && 0 <= tempAvgBright && tempAvgBright < 250) {
        if(tempAvgBright < 40)
            bgBright = tempAvgBright;
        else {
            bgBright = tempAvgBright;
        }
    }


    threshold(grayDst, dst, bgBright, 255, THRESH_TOZERO);
//    openImg(dst);
    Contours contours;

    vector<Vec4i> hierarchy;
    findContours(dst, contours, hierarchy, RETR_LIST, CHAIN_APPROX_NONE);
    vector<Cont> cts;

    for(int i = 0; i < contours.size(); ++i) {
        //drawContours(src, contours, i, Scalar(i * 0x777 % 156, i * 0x888 % 156, i * 0x999 % 156), 3, LINE_8);
        double area = contourArea(contours[i]);
        //qDebug() << "area: " << area;
        //if(area > AREA_GATE)
        if(judgeTap(contours[i], area, grayDst))
            cts.push_back(Cont(contours[i], area ) );
    }

    sort(cts.begin(), cts.end(), ContCmp);
    bool flag = 0;
    vector<Point> tips;
    for(int i = 0; i < cts.size(); ++i) {
        flag = 1;
        Moments mu = moments(cts[i].cont);
        int xc = mu.m10 / mu.m00, yc = mu.m01 / mu.m00;
       // cout << "Cont " << i << " Center at ( " << xc << ", " << yc << ") Area is " << cts[i].area << " ";
        Point lowP = findLowestPoint(cts[i].cont);
        Point cPoint = Point(xc, yc);
        tips.push_back(cPoint);
#ifdef RECORD_DATA
        appendTipsPoints(cPoint.x, cPoint.y);
#endif
#ifdef DEBUG_SHOW
        circle(src, cPoint, 10, Scalar(0, 155, 155), 3);
#endif
    }

    std::vector<Pos2F> worldP2fVec;
    if(flag) {
        for(int i = 0; i < tips.size(); ++i) {
            Point3f worldP = getWorldPos(tips[i].x, tips[i].y);
            Pos2F worldP2f = Pos2F(-worldP.y, worldP.z);
            worldP2fVec.push_back(worldP2f);
        }
    }
    touchManager.handleFrame(worldP2fVec);

#ifdef NOT_USE_LIFE_MANAGE

    vector<Point> tapTips;
    if(flag) {
        if(tips.size() > 0)
            allTips.push_back(tips);
        //cout << endl;
        for(int i = 0; i < tips.size(); ++i) {
            bool isTap = 1;
            for(int k = 0; k < 10; ++k) {
                if(allTips.size()  >= 2 + k) {
                    if(frameId.size() >= k + 1 && totalFrame - frameId[frameId.size() - 1 - k] <= 2) {
                        int index = allTips.size() - 2 - k;
                        for(int j = 0; j < allTips[index].size(); ++j)
                            if(getDis(tips[i].x, tips[i].y, allTips[index][j].x, allTips[index][j].y) < ONE_TIP_GATE) {
                                isTap = 0;
                            }
                    }
                }
            }
            for(int j = 0; j < tapTips.size(); ++j) {
                if(getDis(tips[i].x, tips[i].y, tapTips[j].x, tapTips[j].y) < ONE_TIP_GATE)
                    isTap = 0;
            }
            if(isTap) {
                tapTips.push_back(tips[i]);
                //printf("generate a Tap ");
#ifndef USE_GESTURE
                emit keyTapped(-realP.y, realP.z);
#endif
            }
           // realP.z += 4;
           // realP.z = min(90.0f, realP.z);
            /*
            printf(" (%d, %d) world Pos is (%.2f, %.2f) key is %c\n", tips[i].x, tips[i].y, realP.y, realP.z, (char)keyMap.at<int>(Point(ceil(-realP.y), ceil(realP.z))));
             */
            //printf("Real World (x, y) is (%.2f, %.2f)", -realP.y, realP.z);
        }
#ifdef USE_GESTURE
#ifndef USE_NEW_GESTURE
        if(tapTips.size() > 2) {
            bool allInLeft = 1;
            for(size_t i = 0; i < tapTips.size(); ++i) {
                if(!inLeftRegion(tapTips[i])) {
                    allInLeft = 0;
                    break;
                }
            }
            if(allInLeft) {
                emit gestureTriggered(3);
            }
            else {
                bool allInRight = 1;
                for(size_t i = 0; i < tapTips.size(); ++i) {
                    if(!inRightRegion(tapTips[i])) {
                        allInRight = 0;
                    }
                }
                if(allInRight) {
                    emit gestureTriggered(4);
                }
                else {
                    int leftNum = 0, rightNum = 0;
                    for(size_t i = 0; i < tapTips.size(); ++i) {
                        if(inLeftRegion(tapTips[i]))
                            leftNum++;
                        else if(inRightRegion(tapTips[i]))
                            rightNum++;
                    }
                    if(leftNum > 2 && rightNum > 2)
                        emit gestureTriggered(1);
                    else {
                        for(size_t i = 0; i < tapTips.size(); ++i) {
                            Point3f rp = getWorldPos(tapTips[i].x, tapTips[i].y);
                            emit keyTapped(-rp.y, rp.z);
                        }
                    }
                }
            }
        }
        else {

            for(int i = 0; i < tapTips.size(); ++i) {
                qDebug() << "x " << tapTips[i].x << " y " << tapTips[i].y;
            }

            if(tapTips.size() == 1 && isDeleteKey(tapTips[0])) {
                emit gestureTriggered(2);
            }
            else {
                for(size_t i = 0; i < tapTips.size(); ++i) {
                    Point3f rp = getWorldPos(tapTips[i].x, tapTips[i].y);
                    emit keyTapped(-rp.y, rp.z);
                }
            }
        }
#endif
#endif
    }

#endif

    return flag;
}

void DataKeyboard::findTapPos(int CamIndex)
{
    VideoCapture cam(CamIndex);
    //cam.set(CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
    //cam.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    //cam.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    //cam.set(CV_CAP_PROP_FPS, 60);

    Mat image,dst;
    int imgCount = 0;
    double fps = 0;
    std::string pathPath = "./chessboard_";
//    printf("width = %.2f\n",cam.get(CV_CAP_PROP_FRAME_WIDTH));
//    printf("height = %.2f\n",cam.get(CV_CAP_PROP_FRAME_HEIGHT));
    qDebug() << "width = " << cam.get(CV_CAP_PROP_FRAME_WIDTH) << " height = " << cam.get(CV_CAP_PROP_FRAME_HEIGHT) << " Fps: " << cam.get(CV_CAP_PROP_FPS);
    initKeyBoardMask();
    long long frameCount = 0;
    queue<double> time_q;

    while(!isInterruptionRequested()) {
        cam >> image;

        //std::cout << "(" << image.cols << " , " << image.rows << ")\n";
        if(image.empty()) {
            std::cout << "empty" << std::endl;
            continue;
        }
#ifdef TRANS_CAM
        flip(image, image, -1);
#endif
        frameCount ++;
#ifdef RECORD_DATA
        if(frameCount == 1)
            setStartTime();
#endif


        image.copyTo(dst, KeyBoardMask);

        if(handleImg(dst, fps))
//        if(1)
            frameId.push_back(totalFrame);
        totalFrame++;

        //resize(image, dst, Size(), 0.5, 0.5);
        //imshow("test", dst);
//        int key = waitKey(1);
//        if(key==27)
//            break;
//        else if(key == 's') {
//            imgCount++;
//            imwrite(pathPath + std::to_string(imgCount) + ".png", image);
//            std::cout << "save a Pic\n";
//        }
#ifdef RECORD_DATA
        //videoBuffer.push_back(std::move(image));
        //videoBuffer[videoFrameNum++] = std::move(image);
        //resize(image, videoBuffer[videoFrameNum++], Size(), 1, 1);
#ifdef WRITE_VIDEO
        image.copyTo(videoBuffer[videoFrameNum++]);
#endif
        //image.copyTo(bufferMat);
        //videowriter.write(bufferMat);
        //appendCurrentTime();


#endif
        const int time_q_len = 5;
        if(time_q.size() == time_q_len) {
            fps = 1.0 / ((getTickCount() - time_q.front()) / getTickFrequency() / time_q_len);
//            qDebug() << "Fps:" <<  fps;
            //cout << "Fps " << fps << std::endl;
            time_q.pop();
        }
        time_q.push(getTickCount());
    }
#ifdef RECORD_DATA
    writeIntoFile();
#endif
}
